#include "VxIrrLib.h"
//! Converts a Bullet quaternion to an Irrlicht euler angle
//! angles in radians
void IrrPhysQuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) 
{
	btScalar W = TQuat.getW();
	btScalar X = TQuat.getX();
	btScalar Y = TQuat.getY();
	btScalar Z = TQuat.getZ();
	float WSquared = W * W;
	float XSquared = X * X;
	float YSquared = Y * Y;
	float ZSquared = Z * Z;

	TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
	TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
	TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
	TEuler *= irr::core::RADTODEG;
}
//============================================================================
//! Convert Euler to Quaternion 
//! angles in radians
void IrrPhysEulerToQuaternion( const irr::core::vector3df& v3fEuler, btQuaternion &quat )
{
	btScalar _heading	= v3fEuler.Z * 0.5f;
	btScalar _attitude	= v3fEuler.Y * 0.5f;
	btScalar _bank		= v3fEuler.X * 0.5f;
	btScalar c1 = cos(_heading);
	btScalar s1 = sin(_heading);
	btScalar c2 = cos(_attitude);
	btScalar s2 = sin(_attitude);
	btScalar c3 = cos(_bank);
	btScalar s3 = sin(_bank);
	double c1c2 = c1*c2;
	double s1s2 = s1*s2;
	//w
	quat.setW((btScalar) (c1c2*c3 + s1s2*s3));
	//x
	quat.setX((btScalar) (c1c2*s3 - s1s2*c3));
	//y
	quat.setY((btScalar) (c1*s2*c3 + s1*c2*s3));
	//z
	quat.setZ((btScalar) (s1*c2*c3 - c1*s2*s3));
}

//============================================================================
//! get center of box to furtherest wall to use as radius of physics spher
irr::f32 IrrPhysGetMaxRadius(const irr::core::aabbox3d<irr::f32> &box,const irr::core::vector3df &scale)
{
	irr::core::vector3df ext = box.getExtent();
	irr::f32 ret= ext.X*scale.X;
	if (ext.Y*scale.Y > ret)
	{
		ret=ext.Y*scale.Y;
	}
	if (ext.Z*scale.Z > ret)
	{
		ret=ext.Z*scale.Z;
	}
	return ret;
}



//============================================================================
//! rebuild bounding box from mesh vertexes
irr::scene::IAnimatedMeshSceneNode* RepairBoundingBox(irr::scene::IAnimatedMeshSceneNode* node)
{
   irr::scene::IAnimatedMesh * mesh = node->getMesh();

   irr::core::aabbox3df box;
   for( irr::u32 i = 0; i < mesh->getMeshBufferCount(); i++ )
   {
      mesh->getMesh(1)->getMeshBuffer(i)->recalculateBoundingBox();
      box.addInternalBox(mesh->getMesh(1)->getMeshBuffer(i)->getBoundingBox());
   }
   mesh->setBoundingBox(box);
   node->setMesh(mesh);
   return node;
}
//============================================================================
//! get node position and convert to Bullet btTransform
void IrrPhysGetNodeTransform( irr::scene::ISceneNode* poSceneNode, btTransform& oTransform)
{
  oTransform.setIdentity();

  irr::core::vector3df v3fRot = poSceneNode->getRotation();
  // convert to radians
  v3fRot *= irr::core::DEGTORAD; 
  btQuaternion quatRot;
  IrrPhysEulerToQuaternion( v3fRot, quatRot );
  oTransform.setRotation( quatRot );

  irr::core::vector3df v3fPos = poSceneNode->getPosition();
  btVector3 vec3Pos( v3fPos.X, v3fPos.Y, v3fPos.Z );
  oTransform.setOrigin( vec3Pos );
}
